/*
 *  polygonizer2.h
 *  Fracture
 *
 *  Created by Jamie Portsmouth on 08/05/2011.
 *  Copyright 2011 __MyCompanyName__. All rights reserved.
 *
 */

#ifndef POLYGONIZER2_H
#define POLYGONIZER2_H

#if 0

#include <list>
#include <vector>
#include "Vec4.h"
#include "Matrix3.h"
#include "Aabb.h"
#include "float.h"
#include "Geometry.h"
#include <cmath>

#include "AabbTree.h"


// FOR DEBUGGING PURPOSES
#include <OpenGL/OpenGL.h>
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>


using namespace std;
using namespace Imath;


float ran2(long *idum);

struct ExampleLevelSetFunctor
{
	// Level set at value=0.0
	static float value(Vec4 P)
	{
		float X = P[0];
		float Y = P[1];
		float Z = P[2];
		return X*X + Y*Y + Z*Z - 1.0;
	}
};


class Polygonizer 
{
public:
		
	// Pass the functor which defines the level set as the zero value isosurface
	template<typename Func>
	void polygonize(const Aabb& bbox, const float tolerance);

	const Geometry& getGeometry()
	{
		return m_mesh;
	}
	

	
	struct Front
	{
		// Circular linked list of vertices in the front
		list<int> m_vertices;
	};
		
	vector<Front> m_fronts;
	

	class TriangleBVH
	{
	public:
		
		// Data structure to accelerate collision detection with a mesh which is growing dynamically
		// (i.e. triangles are being appended to it, but not removed).
		// As triangles are inserted, every numTrianglesPerSubTree inserted it places the newly inserted triangles into a two level BVH.
		// The most recently inserted triangles are kept in a small ("dynamic") BVH which is rebuilt on each insertion.
				
		TriangleBVH(const Geometry& mesh, int numTrianglesPerSubTree) : m_mesh(mesh), m_numTrianglesPerSubTree(numTrianglesPerSubTree) {}
		
		~TriangleBVH()
		{
			for (int n=0; n<m_subTrees.size(); ++n)
			{
				delete m_subTrees[n];
			}
		}
		
		// Given a test AABB, return the triangles which it (potentially) overlaps
		void query( const Box3f& testAabb, vector<int>& triangleIndices ) const
		{
			vector<int> leafIndices;
			vector<int> subTreeIndices;
			m_globalTree.query(testAabb, subTreeIndices);
			for (int m=0; m<subTreeIndices.size(); ++m)
			{
				int subTreeIndex = subTreeIndices[m];
				assert(subTreeIndex>=0 && subTreeIndex<m_subTrees.size());
				const AabbTree<int>* subTree = m_subTrees[subTreeIndex];
				subTree->query(testAabb, leafIndices);
				for (int l=0; l<leafIndices.size(); ++l)
				{
					triangleIndices.push_back(subTree->getLeafData(leafIndices[l]));
				}
			}
			{
				m_dynamicTree.query(testAabb, leafIndices);
				for (int l=0; l<leafIndices.size(); ++l)
				{
					triangleIndices.push_back(m_dynamicTree.getLeafData(leafIndices[l]));
				}
			}
		}
				
		// Insert a triangle, which subsequently will be available for overlap queries
		void insertTriangle(int triangleIndex)
		{
			m_latestTriangles.push_back(triangleIndex);
			
			if (m_latestTriangles.size() == m_numTrianglesPerSubTree)
			{
				// Build a new subtree
				AabbTree<int>* subtree = new AabbTree<int>();
				_build(subtree);
				m_subTrees.push_back(subtree);
				m_latestTriangles.clear();
			
				// Rebuild the global tree to include the new subtree
				{
					vector<Box3f> leafAabbs;
					vector<int> leafDatas;
					int M = m_subTrees.size();
					for (int m=0; m<M; ++m)
					{
						const AabbTree<int>* subtree = m_subTrees[m];
						leafAabbs.push_back(subtree->getRootAabb());
						leafDatas.push_back(m);
					}
					m_globalTree.buildTreeTopDown(leafAabbs, leafDatas);
				}
				
				// Empty the dynamic tree
				m_dynamicTree.clear();
			}
			else
			{
				// Rebuild the dynamic tree
				_build(&m_dynamicTree);
			}
		}
		
		private:
		
			const Geometry& m_mesh;
			int m_numTrianglesPerSubTree;
			
			AabbTree<int> m_globalTree; 
			AabbTree<int> m_dynamicTree;
			vector<AabbTree<int>*> m_subTrees;
			
			vector<int> m_latestTriangles;
		
			void _build(AabbTree<int>* tree)
			{
				vector<Box3f> leafAabbs;
				vector<int> leafDatas;
				for (int n=0; n<m_latestTriangles.size(); ++n)
				{
					int triangleIndex = m_latestTriangles[n];
					
					const Geometry::Triangle& T = m_mesh.m_triangles[triangleIndex];
					T.m_vertices[0];
					
					Box3f box;
					box.extendBy(T.m_vertices[0]);
					box.extendBy(T.m_vertices[1]);
					box.extendBy(T.m_vertices[2]);
					leafAabbs.push_back(box);
					leafDatas.push_back(triangleIndex);
				}
				
				tree->buildTreeTopDown(leafAabbs, leafDatas);
			}
	};
	
	//TriangleBVH m_BVH;
	
	
	class EdgeBVH
	{
	public:
		
		// Data structure to accelerate collision detection with a mesh which is growing dynamically
		// (i.e. edges are being appended to it, but not removed).
		// As edges are inserted, every numEdgesPerSubTree inserted it places the newly inserted edges into a two level BVH.
		// The most recently inserted edges are kept in a small ("dynamic") BVH which is rebuilt on each insertion.
		
		EdgeBVH(int numEdgesPerSubTree) : m_numEdgesPerSubTree(numEdgesPerSubTree) {}
		
		~EdgeBVH()
		{
			for (int n=0; n<m_subTrees.size(); ++n)
			{
				delete m_subTrees[n];
			}
		}
		
		// Given a test AABB, return the edges which it (potentially) overlaps
		void query( const Box3f& testAabb, vector<Edge>& edges ) const
		{
			vector<int> leafIndices;
			vector<int> subTreeIndices;
			m_globalTree.query(testAabb, subTreeIndices);
			for (int m=0; m<subTreeIndices.size(); ++m)
			{
				int subTreeIndex = subTreeIndices[m];
				assert(subTreeIndex>=0 && subTreeIndex<m_subTrees.size());
				const AabbTree<Edge>* subTree = m_subTrees[subTreeIndex];
				subTree->query(testAabb, leafIndices);
				for (int l=0; l<leafIndices.size(); ++l)
				{
					edges.push_back(subTree->getLeafData(leafIndices[l]));
				}
			}
			{
				m_dynamicTree.query(testAabb, leafIndices);
				for (int l=0; l<leafIndices.size(); ++l)
				{
					edges.push_back(m_dynamicTree.getLeafData(leafIndices[l]));
				}
			}
		}
		
		// Insert an edge, which subsequently will be available for overlap queries
		void insertEdge(const Edge& edge)
		{
			m_latestEdges.push_back(edge);
			
			if (m_latestEdges.size() == m_numEdgesPerSubTree)
			{
				// Build a new subtree
				AabbTree<Edge>* subtree = new AabbTree<Edge>();
				_build(subtree);
				m_subTrees.push_back(subtree);
				m_latestEdges.clear();
				
				// Rebuild the global tree to include the new subtree
				{
					vector<Box3f> leafAabbs;
					vector<int> leafDatas;
					int M = m_subTrees.size();
					for (int m=0; m<M; ++m)
					{
						const AabbTree<Edge>* subtree = m_subTrees[m];
						leafAabbs.push_back(subtree->getRootAabb());
						leafDatas.push_back(m);
					}
					m_globalTree.buildTreeTopDown(leafAabbs, leafDatas);
				}
				
				// Empty the dynamic tree
				m_dynamicTree.clear();
			}
			else
			{
				// Rebuild the dynamic tree
				_build(&m_dynamicTree);
			}
		}
		
	private:
		
		int m_numEdgesPerSubTree;
		
		AabbTree<int> m_globalTree; 
		AabbTree<Edge> m_dynamicTree;
		vector<AabbTree<Edge>*> m_subTrees;
		
		vector<Edge> m_latestEdges;
		
		void _build(AabbTree<Edge>* tree)
		{
			vector<Box3f> leafAabbs;
			vector<Edge> leafDatas;
			for (int n=0; n<m_latestEdges.size(); ++n)
			{
				const Edge& edge = m_latestEdges[n];
				V3f vA(edge.m_a[0], edge.m_a[1], edge.m_a[2]);
				V3f vB(edge.m_b[0], edge.m_b[1], edge.m_b[2]);
				Box3f box;
				box.extendBy(vA);
				box.extendBy(vB);
				leafAabbs.push_back(box);
				leafDatas.push_back(edge);
			}
			tree->buildTreeTopDown(leafAabbs, leafDatas);
		}
	};
	
	EdgeBVH m_BVH;
	
	
	
	long m_seed;
	
	template<typename Func>
	inline Vec4 gradient(const Vec4& point, float e, float inv2E)
	{
		Vec4 dX = Vec4(e,0,0);
		Vec4 dY = Vec4(0,e,0);
		Vec4 dZ = Vec4(0,0,e);
		float Vxp = Func::value(point+dX);
		float Vxm = Func::value(point-dX);
		float Vyp = Func::value(point+dY);
		float Vym = Func::value(point-dY);
		float Vzp = Func::value(point+dZ);
		float Vzm = Func::value(point-dZ);
		float gradX = inv2E * (Vxp - Vxm);
		float gradY = inv2E * (Vyp - Vym);
		float gradZ = inv2E * (Vzp - Vzm);
		return Vec4(gradX, gradY, gradZ);
	}
	
	template<typename Func>
	inline Matrix3 hessian(const Vec4& point, float e, float inv2E)
	{
		Vec4 dX = Vec4(e,0,0);
		Vec4 dY = Vec4(0,e,0);
		Vec4 dZ = Vec4(0,0,e);
		Vec4 Gxp = gradient<Func>(point + dX, e, inv2E);
		Vec4 Gxm = gradient<Func>(point - dX, e, inv2E);
		Vec4 Gyp = gradient<Func>(point + dY, e, inv2E);
		Vec4 Gym = gradient<Func>(point - dY, e, inv2E);
		Vec4 Gzp = gradient<Func>(point + dZ, e, inv2E);
		Vec4 Gzm = gradient<Func>(point - dZ, e, inv2E);
		
		float hr = 0.5f*inv2E;
		float hXX = inv2E * (Gxp[0] - Gxm[0]);
		float hYY = inv2E * (Gyp[1] - Gym[1]); 
		float hZZ = inv2E * (Gzp[2] - Gzm[2]);
		float hXY = hr * (Gxp[1] - Gxm[1] + Gyp[0] - Gym[0]);
		float hYZ = hr * (Gyp[2] - Gym[2] + Gzp[1] - Gzm[1]);
		float hXZ = hr * (Gzp[0] - Gzm[0] + Gxp[2] - Gxm[2]);
		
		Vec4 col0(hXX, hXY, hXZ);
		Vec4 col1(hXY, hYY, hXY);
		Vec4 col2(hXZ, hYZ, hZZ);
		Matrix3 H;
		H.setColumn(0, col0);
		H.setColumn(1, col1);
		H.setColumn(2, col2);
		return H;
	}
	
	
	template<typename Func>
	Vec4 project(const Vec4& start, float tol, float distanceEpsilon, int maxIterations)
	{
		float inv2D = 0.5f/distanceEpsilon;
		float tolSqr = tol*tol;
		
		Vec4 x = start;
		float f = Func::value(x);
		float fSqr = f*f;

		int nIter = 0;
		
		// Take Newton steps until the function magnitude falls below tolerance
		while (fSqr>tolSqr && nIter<maxIterations)
		{
			// (for the moment, ignore the issue of what to do if the gradient is close to zero).
			Vec4 grad = gradient<Func>(x, distanceEpsilon, inv2D);
			//if ( abs(grad[0]) < tol ) { printf("project() failed, gradient component too small"); exit(1); }
			//if ( abs(grad[1]) < tol ) { printf("project() failed, gradient component too small"); exit(1); }
			//if ( abs(grad[2]) < tol ) { printf("project() failed, gradient component too small"); exit(1); }
			
			// Take a Newton step 
			Vec4 step = f*grad/(grad*grad);
			
			// Reduce step size until the function magnitude falls, if needed 
			float fp, fpSqr;
			Vec4 xp;
			do 
			{
				nIter++;
				xp = x - step;
				fp = Func::value(xp);
				fpSqr = fp*fp;
				step *= 0.5f;
			}
			while (fpSqr>=fSqr && nIter<maxIterations);
			
			x = xp;
			f = fp;
			fSqr = fpSqr;
		}
		
		if (nIter>=maxIterations) 
		{ 
			printf("project() failed, too many iterations"); 
			exit(1); 
		}

		return x;
	}

	
	static inline bool intersectRaySphere(const Vec4& p, const Vec4& d, float R, const Vec4& C)
	{
		Vec4 m = p - C;
		float b = m*d;
		float c = m*m - R*R;
		if (c>0.0f && b>0.0f) return false;
		float discr = b*b - (d*d)*c;
		if (discr<0.0f) return false;
		float tlo = -b - sqrt(discr); 
		float thi = -b + sqrt(discr);
		if (tlo>=0.0f && thi<=d*d) return true;
		return false;
	}
	
	
	static inline void computeCircumcircle(const Vec4& A, const Vec4& B, const Vec4& C,
										   float& circumradius, Vec4& circumcenter)
	{
		// Using formulae from http://en.wikipedia.org/wiki/Circumscribed_circle
		Vec4 a = A - C;
		Vec4 b = B - C;
		float al = a.length3();
		float bl = b.length3();
		Vec4 acrossb = a % b;
		circumradius = al * bl * (a-b).length3() / (2.f * acrossb.length3());
		circumcenter = C + (((al*al)*b - (bl*bl)*a) % acrossb) / (2.f * (acrossb*acrossb));
	}
	
	template<typename Func>
	bool grow()
	{
		return grow<Func>(m_fronts[0], m_mesh, m_distanceEpsilon, m_tol);
	}
	

	void drawFronts()
	{
		glPushAttrib(GL_ALL_ATTRIB_BITS);
		
		glDisable(GL_TEXTURE_2D);
		glEnable(GL_DEPTH_TEST);
			
		glDisable(GL_CULL_FACE);
		glCullFace(GL_BACK);
		glEnable(GL_BLEND);
		glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
		
		Vec4 color(1,0,0,1);
		glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, &color[0]);
		glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, &color[0]);
		
		glDisable(GL_LIGHTING);
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
	
		for (int f=0; f<m_fronts.size(); ++f)
		{
			Front& front = m_fronts[f];
			
			glColor4f(1,0,0,0.5);
			glBegin(GL_LINE_LOOP);

			list<int>::iterator vertex = front.m_vertices.begin();
			while (vertex != front.m_vertices.end())
			{
				const Vec4& V = m_mesh.m_vertices[vertex];
				glVertex3fv(&V[0]);
				vertex++;
			}
			
			glEnd();
		}

		glPopAttrib();
	}
	
	
	template<typename Func>
	bool grow(Front& front, Geometry& mesh, float distanceEpsilon, float tol)
	{		
		// Find "best" front vertex.
		
		
		
		list<int>::iterator frontVertex = m_currentFrontVertex; //front.m_edges.begin();
		if (boundaryTriangle == front.m_boundaryTriangles.end())
		{
			boundaryTriangle++;
		}
		
		// Deal with one vertex at a time (for debugging)
		{
			
			
			int a = boundaryTriangle->m_a;
			int b = boundaryTriangle->m_b;
			int c = boundaryTriangle->m_otherTriangleVertex;
			
			const Vec4& A = mesh.m_vertices[boundaryTriangle->m_a];
			const Vec4& B = mesh.m_vertices[boundaryTriangle->m_b];
			const Vec4& C = mesh.m_vertices[boundaryTriangle->m_otherTriangleVertex];

			// Construct a new candidate point
			Vec4 midPoint = 0.5f * (A + B);
			Vec4 centroid = 0.3333333f * (A + B + C);
			
			Vec4 n = (centroid - A) % (B - A);
			Vec4 outVec = n % (B - A);
			outVec.normalizeIfNotZero();
			
			// This is where we should be intelligently choosing a good length based on 
			// the local curvature ...   for now, just do:
			float extrapolationLength = sqrt(3.f)/2.f * (B - A).length3();		
			
			Vec4 candidatePoint = midPoint + extrapolationLength*outVec;

			// Project it to the isosurface
			const int maxIterations = 100;
			Vec4 projectedPoint = project<Func>(candidatePoint, tol, distanceEpsilon, maxIterations);
			

			
		
		}
		
		m_currentBoundaryTriangle = boundaryTriangle;
		
		// We now have gone as far as we can from this seed, without pasting the cracks.
	
		return triangleAdded;
	}
	
	
	Polygonizer() : m_seed(-10), m_BVH(m_mesh, 8)
	{
		ran2(&m_seed);	
	}
	
	
	Geometry m_mesh;
	
	float m_distanceEpsilon;
	float m_tol;
	
};


/*
 
	Vertex-based algorithm:
 
		- maintain fronts with vertex linked lists.  
 
		- add points using an angle criterion to try to keep triangles equilateral

		- we split/merge fronts when overlaps are detected. Overlap detection is done as follows:
 
 			- As we build the mesh, each vertex is "fattened" by the local radius of curvature (r.o.c).  (/2 ?)
			  This defines fattened triangles by barycentric interpolation. We keep a BVH of the fattened triangles.
 
			- Check the current front vertex V the following way:
 
				- Make all of the candidate points for the front (i.e. construct the local fan at V).
				- For each candidate vertex P, do the the following tests:

					- Do BVH test of point P fattened by its local r.o.c rP  (/2 ?), and the fattened triangle AABBs.
 
					- For each triangle T hit:
		
						- Ignore any triangle which contains the vertex V (it is easily seen that these cannot be true overlaps)
						- Check whether the vertex actually lies inside the fat triangle T, via:
 
								- compute closest point C of P on T, and barycoordinates of that point, w_i.
								- compute r(C) = sum_i w_i r_i,  where r_i is the r.o.c at the ith vertex.
								- isInside = |P - C| < r(C) + rP
						- If isInside, the candidate point is unacceptable.
 
					- Additionally, test whether the edge between the point P and vertex V intersects any mesh edge,
					 using "projected edge collision check". This requires maintaining a BVH of mesh edges.
 
					- If the point P fails either test, break.
 
			- If all candidate points are acceptable, we can go ahead and expand the mesh, update the BVH, update the fronts and continue.
 
			- Otherwise, we must split or merge at the vertex V.
 
					- Find closest vertex to P (first candidate point that failed) iteratively (ignoring V and its neighbours). Call this V'.
					- Do split or merge depending on whether V' is on the same front or a different front.
					- Now we attempt to connect V and V'. 
							- If the edge V-V' is 'valid', we can generate the new fronts and proceed.
						      ['Validity' test is done by doing a projected edge collision check.]
							- If is not valid (rare?), try the next closest point, V'', and so on.
							- [What if we exhaust all points? Perhaps this is not possible, since we are doing something like
							   exhaustive triangulation of a polygon projected onto a surface ...   not obvious...]
 
 
 
 
	Vertex based algorithm 2:   [single front here, for simplicity - later extend to multiple disconnected seed fronts]
 
 
		required:
 
			- this algorithm uses edge-edge based collision detection. However, these edges are not line segments in 3d, but 
			  line segments in 3d projected onto the 2d isosurface.
 
 
		struct ProjectedEdge
		{
			// A projected edge defined by splitting the 3d line segment A-B into numSegments sub-segments, 
			// then projecting the endpoints of each onto the isosurface.
			ProjectedEdge(V3f A, V3f B, int numSegments=16);
 
			private:
 
				V3f m_endpointA;
				V3f m_endpointB;
				vector<V3f> m_interiorPoints;
				BVH m_segmentBVH;
		}
 
		// Returns true if the closest points of the two ProjectedEdges are closer than the distanceTolerance
		bool intersection( const ProjectedEdge& E0, const ProjectedEdge& E1, float distanceTolerance );
 
		
 
 
 
		GROWING PHASE
 
		- maintain fronts with vertex linked lists.  
 
		- generate candiate vertices from existing front vertices, using:
 
				- the local r.o.c as the edge size heuristic
				- an angle criterion to try to keep triangles equilateral
 
		- overlap detection is edge-based. 
 
				- we rebuild the BVH of the entire set of front edges whenever the front changes, 
				  and the bounding boxes of the edge endpoints are expanded by their local r.o.c.
				
				- whenever we try to add a new set of vertices at an existing vertex (and the corresponding new mesh edges), we make the expanded new edges, and check
                  for overlap of each new expanded edge with the existing front (expanded-)edge BVH.  
 
						- NB, new expanded edges will obviously hit the expanded edges formed by the connections of the existing vertex.
                          These edges are therefore ignored in the overlap test.
 
				- (though first, check whether the candidate vertex is outside the bounding box. If so, don't add it, and mark the vertex
				  as a boundary vertex, which is subsequently skipped (during the growing phase).
 
				- if we get overlap, do the narrowphase by getting the closest points on the two unexpanded edges, then 
				  we consider it a hit if the distance between the points is less than the sum of the r.o.c at the points
                  (where the r.o.c. at the points is just obtained by linear interpolation of the r.o.c at the endpoint vertices).
				
		- we add as many points as we can until either overlap prevents adding any more. The remaining front points are obviously on the crack.
 
		- routines required:
 
				float rocAtPoint();
				float openingAngleAtFrontVertex()
				
 
 
 
		CRACK FILLING PHASE
	
		- then we have to paste the cracks.  This is done in O(n^2) as follows:
 
			- first build a BVH of the bboxes of all the edges on the front
			- then do a form of ear-clipping based triangulation:

					- n = number of vertices in front
					- first flag whether each vertex is an ear tip (i.e. v(i-1)<->v(i+1) is diagonal - using BVH and egge-edge narrowphase)
						
					while n>3:
						
						find a vertex which is flagged as an ear, vi
						add a new triangle by creating the diagonal v(i-1)<->v(i+1)
						delete vi from the front
						recompute the ear tip status of v(i-1), v(i+1)
						
		- after this phase the mesh should be complete.
 
		- routines required:
 
				bool isDiagonal(A, B)
 
				
*/

inline void computeTangentSpace(const Vec4& N, Vec4& T, Vec4& B)
{
	// (N assumed normalized)
	float nx = N[0];
	float ny = N[1];
	float nz = N[2];
	if (nx>0.5f || ny>0.5f) 
		T = Vec4(ny, -nx, 0.f);
	else
		T = Vec4(-nz, 0.f, nx);
	B = N % T;
	T.normalizeIfNotZero();
	B.normalizeIfNotZero();
}






template<typename Func>
void Polygonizer::polygonize(const Aabb& bbox, const float tol)
{
	// Look for a good start point
	Vec4 extents = bbox.m_maxima - bbox.m_minima;
	const int startPointSearchIters = 32*32*32;
	float minValue = FLT_MAX;
	Vec4 startPoint;
	for (int n=0; n<startPointSearchIters; ++n)
	{
		Vec4 testPoint = bbox.m_minima + ran2(&m_seed) * extents;
		float testVal = fabs(Func::value(testPoint));
		if ( testVal<minValue ) 
		{
			startPoint = testPoint;
			minValue = testVal;
		}
	}
	
	m_tol = tol;
	m_distanceEpsilon = tol * extents.length3();
	const int maxIterations = 100;
	
	// Project it to the isosurface
	Vec4 seedPoint = project<Func>(startPoint, tol, m_distanceEpsilon, maxIterations);
	
	float inv2D = 0.5f/m_distanceEpsilon;
	Vec4 grad = gradient<Func>(seedPoint, m_distanceEpsilon, inv2D);
	
	Vec4 n = grad;
	n.normalizeIfNotZero();
	
	Vec4 t, b;
	computeTangentSpace(n, t, b);
	
	// Compute first fundamental form
	float E = t * grad; 
	float G = b * grad; 
	float F = E * G;
	E *= E; G *= G;
	
	// Compute second fundamental form
	Matrix3 H = hessian<Func>(seedPoint, m_distanceEpsilon, inv2D);
	
	float L = t * (H * t);
	float N = b * (H * b);
	float M = t * (H * b);
	
	// Compute principal curvatures
	float m00 = L*G - M*F;
	float m01 = M*G - N*F;
	float m10 = M*E - L*F;
	float m11 = N*E - M*F;
	
	float trace = m00+m11;
	float radical = trace*trace - 4.f*(m00*m11-m01*m10);
	if (radical<0.0f)
	{
		// TO DO: deal with this
		printf("Unable to diagonalize shape operator."); 
		exit(1);
	}
	
	radical = sqrt(radical);
	float k1 = 0.5f * (trace + radical);
	float k2 = 0.5f * (trace - radical);
	float meanCurvature = 0.5f * (k1 + k2);
	
	// thus compute candidate triangles for expansion
	
	/////////////////////// DEBUG ///////////////////////
	float radiusOfCurvature = 0.05f; //1.f/meanCurvature;
	/////////////////////// DEBUG ///////////////////////
	
	// stamp initial hexagon
	m_mesh.m_vertices.push_back(seedPoint);
	
	// Generate initial front
	for (int i=0; i<6; ++i)
	{
		float x = radiusOfCurvature * cos(float(i) * 0.3333333f*M_PI);
		float y = radiusOfCurvature * sin(float(i) * 0.3333333f*M_PI);
		m_mesh.m_vertices.push_back(seedPoint + x*t + y*b);
		
		// TO DO:  normals should be computed at each mesh vertex too, as we grow.
	}
	
	Front front;
	for (int i=0; i<6; ++i)
	{
		front.m_vertices.push_back(1+(i%6));
	}
	m_fronts.push_back(front);

	for (int i=0; i<6; ++i)
	{
		Geometry::Triangle tri;                                                                                                                       
		tri.m_vertex[0] = 0;
		tri.m_vertex[1] = 1+(i%6);
		tri.m_vertex[2] = 1+((i+1)%6);
		m_mesh.m_triangles.push_back(tri);
		//const Vec4& A = m_mesh.m_vertices[tri.m_vertex[0]];
		//const Vec4& B = m_mesh.m_vertices[tri.m_vertex[1]];
		//const Vec4& C = m_mesh.m_vertices[tri.m_vertex[2]];
		//m_BVH.insertTriangle(m_mesh.m_triangles);
	}
	
	// DEBUG
	
	//m_currentBoundaryTriangle = m_fronts[0].m_boundaryTriangles.begin();

	// loop:
	
	
	// Iterate over patches P:
	
	//		Iterate over each front F of patch P:
	
	//				Iterate over vertices V of F
	
	//					Attempt to 'expand' front at vertex V (make edge including V, make triangle from other point 
	//					at candidate place based on curvature + tangent plane + projection)
	
	//						If we find that this would lead to an intersection (or close collision) with some patch Q 
	//						(based on some conservative critertion):
	
	//								If the intersected patch Q != P,  merge fronts of Q, merge patches
	//								If the intersected patch Q == P,  split fronts of Q [do not split patch]
	
	//								[+ potentially logic to prevent problems if multiple collisions happen ...]
	//								[+ special case to eliminate the 'final' three vertex fronts ...]
	                        
	
	// At the end of this, all fronts should have been merged, and shrunk to 3 vertex fronts, then eliminated completely,
	// leaving a single patch with no boundaries.  This is your polygonized isosurface...
	                                                                                                                                                                             
	
}
          

#endif

#endif //POLYGONIZER2_H






